/*
 * ManipulatorSystem.h
 *
 *  Created on: Jan 13, 2011
 *      Author: Robotics
 */

#ifndef MANIPULATORSYSTEM_H_
#define MANIPULATORSYSTEM_H_


#include "Victor.h"
#include "AnalogChannel.h"
#include "PIDController.h"
#include "DriverStationEnhancedIO.h"
#include "DigitalInput.h"
#include "DoubleSolenoid.h"
#include "KHSJoystick.h"
#include "CoordinateMath.h"

//Change the length to match up with the bot specs
#define Uprightl 54;


class ManipulatorSystem{


public:


	ManipulatorSystem();
	~ManipulatorSystem();


	double getEncoderValue();
	void ManipulatorSystem::Autonomous(int, int);
	void ManipulatorSystem::DriverControlGlove();
	void ManipulatorSystem::DriverControlJoystick(KHSJoystick*);
	double ManipulatorSystem::Joint1PotToAngle();
	double ManipulatorSystem::Joint1AngleToPot(float);
	double ManipulatorSystem::Joint2PotToAngle();
	double ManipulatorSystem::Joint2AngleToPot(float);
	Point3D ManipulatorSystem::GetCameraCoordinates();
	void ManipulatorSystem::disable();
	bool setArmJoints(float, float);
	void setClamped(bool);
	void calibrate(KHSJoystick*);
	void calibrateArm(KHSJoystick* );
	void reloadCalibration();


    Point3D m_clawPosition;
	
private:

	Victor m_Victor1, m_Victor2;
	AnalogChannel m_AnalogChannel1, m_AnalogChannel2;
	PIDController m_PID1, m_PID2;
	DriverStation *mptr_DriverStation;
    DoubleSolenoid m_Valve;
    bool m_PneumaticsFlag;
    int m_previousDigitalIn;
    float m_preverr;
	float m_J1Min, m_J1Max, m_J2Min, m_J2Max, m_J1M, m_J2M, m_J1Y, m_J2Y, m_GJ1Max, m_GJ1Min, m_GJ2Max, m_GJ2Min;








};

#endif /* MANIPULATORSYSTEM_H_ */




